[ROS学习]Euroc数据集真实轨迹的读取与显示

您所在的位置：网站首页 › leica fotos21下载 › [ROS学习]Euroc数据集真实轨迹的读取与显示

[ROS学习]Euroc数据集真实轨迹的读取与显示

2024-01-12 09:21| 来源: 网络整理| 查看: 265

Euroc数据集真实轨迹的读取与显示 1. Euroc数据集真实轨迹数据(csv格式)的读取1.1 Euroc数据集1.2 Euroc真实轨迹 groundtruth的格式数据频率为200hz1.3 csv数据的读取**数据读取基础****读取流程(代码设计)** 1.4 ros中读取csv并发布 2 真实轨迹数据在rviz中的显示2.1 数据显示在rviz中的逻辑2.2 整体程序设计2.2 rviz中的设置参考

1. Euroc数据集真实轨迹数据(csv格式)的读取 1.1 Euroc数据集

Euroc数据集是用于室内MAV的双目+IMU数据集，包含两个场景

（1）机器仓库

（2）普通房间

硬件设备

飞行器机体：AscTec Firefly双目VIO相机：全局快门，单色，相机频率20Hz，IMU频率200Hz，具备相机和IMU的硬件(hw)同步，双目相机型号MT9V034，IMU型号ADIS16448VICON0：维肯动作捕捉系统的配套反射标志，叫做markerLEICA0：是激光追踪器配套的传感器棱镜，叫做prismLeica Nova MS50: 激光追踪器，测量棱镜prism的位置，毫米精度，帧率20Hz，Vicon motion capture system: 维肯动作捕捉系统，提供在单一坐标系下的6D位姿测量，测量方式是通过在MAV上贴上一组反射标志，帧率100Hz，毫米精度

关于坐标系变换可参考文献[1]，或者数据集官方页面

1.2 Euroc真实轨迹 groundtruth的格式数据频率为200hz

(1)18位时间戳

timestamp，最小单位为纳秒

(2)当前MAV的三维空间坐标[position]，R代表该信息是由Leica Nova MS50(激光追踪器)的信息在R坐标系下的值。

S表示Sensor传感器坐标下的信息，因此RS_R表示由本来的信息在S坐标系下，后变换到了R坐标系。

单位[m]

p_RS_R_xp_RS_R_yp_RS_R_z

(3)当前位姿[quaternion形式] 其中w为四元数的实部，xyz为三个虚部

q_RS_w

q_RS_x

q_RS_y

q_RS_z

(4)当前速度，单位[m/s]

v_RS_R_xv_RS_R_yv_RS_R_z

(5)当前角速度，单位[rad/s]

b_w_RS_S_xb_w_RS_S_yb_w_RS_S_z

(6)当前线速度，单位[m/s]

b_a_RS_S_xb_a_RS_S_yb_a_RS_S_z 1.3 csv数据的读取数据读取基础

文件指针

// 打开路径path下的文件 std::ifstream data(path, std::ios::in);

文件指针打开失败判断

if(!data.is_open()) { std::cout std::ifstream data("${Your_path}/data.csv", std::ios::in); std::string line; std::getline(data, line); if(!data.is_open()) { std::cout std::cout std::cout //设置编码 setlocale(LC_ALL,""); //2.初始化 ROS 节点:命名(唯一) // 参数1和参数2 后期为节点传值会使用 // 参数3 是节点名称，是一个标识符，需要保证运行后，在 ROS 网络拓扑中唯一 ros::init(argc,argv,"pub_ground_truth_data"); //3.实例化 ROS 句柄 ros::NodeHandle nh;//该类封装了 ROS 中的一些常用功能 //4.实例化发布者对象 //泛型: 发布的消息类型 //参数1: 要发布到的话题 //参数2: 队列中最大保存的消息数，超出此阀值时，先进的先销毁(时间早的先销毁) ros::Publisher pub = nh.advertise("chatter",10); //5.组织被发布的数据，并编写逻辑发布数据 //数据(动态组织) // 创建数据读取对象 std::vector data_raw; data_raw = readcsvdata("${Your_path}/data.csv"); int count = 0; //消息计数器 //逻辑(一秒10次) // 参数 freq ros::Rate r(100); // 保持节点生存周期 while (ros::ok()) { //节点终止条件 if(count >= data_raw.size()) break; //发布消息 pub.publish(data_raw[count]); //加入调试，打印发送的消息 ROS_INFO("已发送的id: %d, timestamp: %.2f, x: %f, y: %f, z: %f", count,data_raw[count].timestamp, data_raw[count].p_RS_R_x, data_raw[count].p_RS_R_y, data_raw[count].p_RS_R_z); //根据前面制定的发送频率自动休眠休眠时间 = 1/频率； r.sleep(); count++;//循环结束前，让 count 自增 ros::spinOnce(); } return 0; } 2 真实轨迹数据在rviz中的显示 2.1 数据显示在rviz中的逻辑

使轨迹数据(坐标+位姿)在rviz中显示的关键是将(坐标+位姿)的数据以rviz可以订阅的消息类型进行发布。

ROS的数据在rviz中显示的接口主要在 geometry_msgs与 nav_msgs中

其中 nav_msgs::path可以显示轨迹路径

// 轨迹的发布 ros::Publisher path_pub = ph.advertise("trajectory", 1, true); // 获取当前时间戳 ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); // 创建rviz数据对象 nav_msgs::Path path; //nav_msgs::Path path; path.header.stamp = current_time; // 指定数据帧的标头 path.header.frame_id = "odom"; // 创建rviz几何发布对象 geometry_msgs::PoseStamped this_pose_stamped; // 创建rviz几何发布对象 geometry_msgs::PoseStamped this_pose_stamped; // 轨迹信息 this_pose_stamped.pose.position.x = data_raw[count].p_RS_R_x; this_pose_stamped.pose.position.y = data_raw[count].p_RS_R_y; this_pose_stamped.pose.position.z = data_raw[count].p_RS_R_z; // 四元数的四个信息一个实部、三个虚部 this_pose_stamped.pose.orientation.x = data_raw[count].q_RS_R_x; this_pose_stamped.pose.orientation.y = data_raw[count].q_RS_R_y; this_pose_stamped.pose.orientation.z = data_raw[count].q_RS_R_z; this_pose_stamped.pose.orientation.w = data_raw[count].q_RS_R_w; this_pose_stamped.header.stamp = current_time; this_pose_stamped.header.frame_id = "odom"; path.poses.push_back(this_pose_stamped); // 发布 path_pub.publish(path);

geometry_msgs可以显示点的位置

// 点位置发布 ros::Publisher point_pub = ph.advertise("point", 1, true); // 获取当前时间戳 ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); // 创建rviz几何发布对象 geometry_msgs::PointStamped this_point_stamped; //点信息 this_point_stamped.header.stamp = current_time; this_point_stamped.header.frame_id = "odom"; this_point_stamped.point.x = data_raw[count].p_RS_R_x; this_point_stamped.point.y = data_raw[count].p_RS_R_y; this_point_stamped.point.z = data_raw[count].p_RS_R_z; // 发布 point_pub.publish(this_point_stamped); 2.2 整体程序设计

整体的程序设计分为三个部分：

csv数据读取创建发布对象、整合发布数据数据发布

部分库与csv读取函数参考上文

#include #include #include #include #include #include #include #include #include #include int main(int argc, char **argv) { ros::init(argc, argv, "my_trace"); // 从数据集的state_groundtruth_estimate0读取真实轨迹 std::vector data_raw; data_raw = readcsvdata("${Your_path}/data.csv"); // 创建发布对象 ros::NodeHandle ph; // 轨迹的发布 ros::Publisher path_pub = ph.advertise("trajectory", 1, true); // 点位置发布 ros::Publisher point_pub = ph.advertise("point", 1, true); // 获取当前时间戳 ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); // 创建rviz数据对象 nav_msgs::Path path; //nav_msgs::Path path; path.header.stamp = current_time; // 指定数据帧的标头 path.header.frame_id = "odom"; // 计数,当count达到数据的最大id时结束发布进程 int32_t count = 0; // 设置数据发送频率 ros::Rate loop_rate(100); while (ros::ok()) { // 是否结束发布进程 if(count >= data_raw.size()) break; current_time = ros::Time::now(); // 创建rviz几何发布对象 geometry_msgs::PoseStamped this_pose_stamped; geometry_msgs::PointStamped this_point_stamped; this_pose_stamped.pose.position.x = data_raw[count].p_RS_R_x; this_pose_stamped.pose.position.y = data_raw[count].p_RS_R_y; this_pose_stamped.pose.position.z = data_raw[count].p_RS_R_z; this_point_stamped.header.stamp = current_time; this_point_stamped.header.frame_id = "odom"; this_point_stamped.point.x = data_raw[count].p_RS_R_x; this_point_stamped.point.y = data_raw[count].p_RS_R_y; this_point_stamped.point.z = data_raw[count].p_RS_R_z; ROS_INFO("current_x: %f", data_raw[count].p_RS_R_x); ROS_INFO("current_y: %f", data_raw[count].p_RS_R_y); this_pose_stamped.pose.orientation.x = data_raw[count].q_RS_R_x; this_pose_stamped.pose.orientation.y = data_raw[count].q_RS_R_y; this_pose_stamped.pose.orientation.z = data_raw[count].q_RS_R_z; this_pose_stamped.pose.orientation.w = data_raw[count].q_RS_R_w; this_pose_stamped.header.stamp = current_time; this_pose_stamped.header.frame_id = "odom"; path.poses.push_back(this_pose_stamped); path_pub.publish(path); point_pub.publish(this_point_stamped); count++; ros::spinOnce(); // check for incoming messages last_time = current_time; loop_rate.sleep(); } return 0; }

需要注意的点是创建功能包时需要导入如下几个功能包

nav_msgsroscppsensor_msgsstd_msgstf 2.2 rviz中的设置

在终端中输入rviz打开rviz的GUI

(1)修改 Global Option中的 Fixed Frame由map 改为 odom 具体看轨迹发布程序中 frame id设置的是什么，本文这里设置是 odom

在这里插入图片描述